#include "move.h"
#include "linked.h"
#include "timer.h"

extern struct LinkedStruct cycle[8];
extern struct LinkedStruct* currLeft;
extern struct LinkedStruct* currRight;
extern int stepVals[] = {5, 4, 6, 2, 10, 8, 9, 1};

void init_movement(void)
{
	int i = 0;

	for(;i<8;i++)
	{
		cycle[i].val = stepVals[i];
		cycle[i].next = &cycle[(i+1)%8];
		cycle[i].prev = &cycle[(i+8-1)%8];
	}

	currLeft = cycle;
	currRight = cycle;
	DDRT = 0xFF;      // PTT7-4 are left track, PTT3-0 are right track
	PTT = (currLeft->val << 4) | currRight->val;//0x55;       // activate two track stepper coils
}

void f_step_forward(void)
{
	currLeft = (((currLeft-cycle)/sizeof(struct LinkedStruct))%2)==0?currLeft->next->next:currLeft->next;
	currRight = (((currRight-cycle)/sizeof(struct LinkedStruct))%2)==0?currRight->next->next:currRight->next;
	PTT = (currLeft->val << 4) | currRight->val;
}
void f_step_backward(void)
{
	currLeft = (((currLeft-cycle)/sizeof(struct LinkedStruct))%2)==0?currLeft->prev->prev:currLeft->prev;
	currRight = (((currRight-cycle)/sizeof(struct LinkedStruct))%2)==0?currRight->prev->prev:currRight->prev;
	PTT = (currLeft->val << 4) | currRight->val;
}
void h_step_forward(void)
{
	currLeft = currLeft->next;
	currRight = currRight->next;
	PTT = (currLeft->val << 4) | currRight->val;
}
void h_step_backward(void)
{
	currLeft = currLeft->prev;
	currRight = currRight->prev;
	PTT = (currLeft->val << 4) | currRight->val;
}
void rotate(short deg)
{
	int i = 0;

	if(deg < 0)
	{
		for(;i>deg;i-=3)
		{
			currLeft = currLeft->next;
			currRight = currRight->prev;	
			PTT = (currLeft->val << 4) | currRight->val;
			wait(MOVEWAIT);
		}
	}
	else if(deg > 0)
	{
		for(;i<deg;i+=3)
		{
			currLeft = currLeft->prev;
			currRight = currRight->next;	
			PTT = (currLeft->val << 4) | currRight->val;
			wait(MOVEWAIT);
		}
	}
}

